//----------------------------------------------------------------------------
//
// (C) Copyrights MARIN/MSCN  2004 
//
//----------------------------------------------------------------------------
//
// $Source: $
//
// $State: Exp $
//
// $Locker:  $
//
// $Revision: 1.1 $
//
// $Date: 2002/06/11 13:59:47 $
//
// $Author: mscn $
//
// $Contractor: $
//
// $Project: $
//
// $Log: $
//
// $Description: $
//
// $End: $
//
//----------------------------------------------------------------------------

#ifndef __mml_Line3_h__
#define __mml_Line3_h__

#include "api.h"
#include <mst/Math.h>
#include <mst/Vector3.h>
#include "Plane.h"
#include <iostream>

namespace mml {

/*! 
\class Line3 mml/Line3.h
\brief 3D line template class
\ingroup mmllin
*/

template<class Type> class Line3 {
public:
	/*!
	\typedef typename mst::Vector3<Type> Vector
	\brief conveniant vector type definition
	*/
        typedef typename mst::Vector3<Type> Vector;

public:
	/*!
		\fn Line3()
		\brief Construct a 3D line
	*/
	Line3() { }
	/*!
		\fn Line3(const Vector &v1, const Vector &v2)
		\brief Construct a 3D line
		\param v1 the vector1 initialisation value
		\param v2 the vector2 initialisation value
	*/
	Line3(const Vector &v1, const Vector &v2) { set(v1, v2); }

	/*!
		\fn void get(Vector &v1, Vector &v2) const
		\brief Get the values of the line
		\param v1 the vector1 value
		\param v2 the vector2 value
	*/
	void get(Vector &v1, Vector &v2) const { v1= m_v[0]; v2 = m_v[1]; }
	/*!
		\fn void set(Vector v1, Vector v2)
		\brief Set the value of the line
		\param v1 the vector1 initialisation value
		\param v2 the vector2 initialisation value
	*/
	void set(Vector v1, Vector v2) { m_v[0] = v1; m_v[1] = v2; }
	/*!
		\fn Vector &operator[](int i)
		\param i index
		\brief Accesses indexed component of line
	*/
	Vector &operator[](int i) { return m_v[i]; }
	
	/*!
		\fn const Vector &operator[](int i) const
		\param i index
		\brief Accesses indexed component of line
	*/
	const Vector &operator[](int i) const { return m_v[i]; }
	/*!
		\fn void copy(const Line3<Type> &l)
		\param l Line to copy
		\brief Copy line
	*/
	void copy(const Line3<Type> &l) { set(l[0], l[1]); }
	/*!
		\fn bool equal(const Line3<Type> &l) const
		\param l The line
		\brief Test for component-wise equality
	*/
	bool equal(const Line3<Type> &l) const {
		return ( (m_v[0].equal(l[0]) && m_v[1].equal(l[1])) ||
			 (m_v[0].equal(l[1]) && m_v[1].equal(l[0])) );
	}
	/*!
		\fn bool operator==(const Line3<Type> &l)
		\param l The line
		\brief Test for equality of a line with the current line
	*/
	bool operator==(const Line3<Type> &l) { return equal(l); }
	/*!
		\fn bool operator!=(const Line3<Type> &l)
		\param l The line
		\brief Test for inequality of a line with the current line
	*/
	bool operator!=(const Line3<Type> &l) { return !equal(l); }

public:	// Warning: return by value can be slow
	/*!
		\fn Line3<Type>& operator=(const Line3<Type>& l)
		\param l The 3D line
		\brief Assign a line
	*/
	Line3<Type>& operator=(const Line3<Type>& l) {
		set(l[0], l[1]);
		return *this;
	}

	/*!
		\fn bool intersection(const Line3<Type>& l, Vector &ip)
		\sa intersection
		\param l line to intersect with
		\param ip The intersection point
		\brief Calculate intersection of 2 lines

		\f$ P = P_1 + \mu_1 (P_2 - P_1) \f$

		\f$ P = P_3 + \mu_2 (P_4 - P_3) \f$

		where \f$ P_i = (x_i, y_i, z_i) \f$

		when lines are co-planar \f$ (c \cdot (a \times b) = 0) \f$

		\f$ P  = P_1 +  a \cdot \frac{(c \times b)(a \times b)} 
		{ |a \times b|^2} \f$

		where \f$ a = P_2 - P_1 \f$
		      \f$ b = P_4 - P_3 \f$
		      \f$ c = P_3 - P_1 \f$

	*/
	bool intersection(const Line3<Type>& l, Vector &ip) 
	{
		int res = false;
		Vector point;
		Vector a, b, c;
		Vector ab, cb;
		Type scale; 

		a.sub(m_v[1], m_v[0]);
		b.sub(l[1], l[0]);
		c.sub(l[0], m_v[0]);

		ab.cross(a,b);

		if (c.dot(ab) == 0) {
			if (ab.dot(ab) != 0.0 ) {
			
				cb.cross(c, b);		
				scale = cb.dot(ab)/ab.dot(ab);
				point = m_v[0] + a*scale;
				ip.set(point);
				res = true;
			}
		}

		return res;
	}

       	/*!
		\fn bool segmentIntersection(const Plane<Type>& plane, Vector &ip, bool isRay)
		\sa intersection
		\param plane The plane to intersect with
		\param ip The intersection point
                \param isRay The line segment is a ray, going on indefinitely at the second endpoint
		\brief Calculate intersection of line with plane

		\f$ P = P_1 + \mu (P_2 - P_1) \f$

		\f$ N \cdot (P - P_3) = 0 \f$

		\f$ \mu = \frac{ N \cdot (P_3 - P_1) }  { N \cdot (P_2 - P_1) } \f$

		where \f$ P_i = (x_i, y_i, z_i) \f$

		and \f$ N = normal \f$

		when line and plane are co-planar \f$ (N \cdot (P_2 - P1) = 0) \f$

	*/
	bool segmentIntersection(const Plane<Type>& plane, Vector &ip, bool isRay=false) 
	{
		bool res = false;
		const Vector sub(m_v[1]-m_v[0]); 
		Type mu(plane[3]);
		Type normalDotSub;
		const Vector planeNormal = plane.normal();

		normalDotSub = planeNormal.dot(sub);

		// check on co-planarity of line and plane
		if (normalDotSub != 0.0) {
			// element 3 (4th element of plane is dot product of normal and a point on plane)
			// mu -= planeNormal.dot(m_v[0]) / normalDotSub;
			mu = (mu - planeNormal.dot(m_v[0])) / normalDotSub;
			if (mu >= 0.0 && (isRay || mu <= 1.0)) {
				ip.addScaled(m_v[0], mu, sub);
				res = true;
			}
		}

		return res;
	}
        
        bool shortestLine(const mml::Line3<Type> &line2, mml::Line3<Type> *pLine)
        // The shortest line between two lines in 3D by Paul Bourke (April 1998)
        // http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline3d/
        // input : other line
        // output: resulting line 
        // result: true if the lines have a shortest line between them
        // adapted and added to mml by Anneke Sicherer-Roetman
        {
            const double SMALL_NUM = 0.00000001;
            mst::Vector3<Type> p1, p2, p3, p4;
            get(p1, p2);
            line2.get(p3, p4);
            mst::Vector3<Type> p13, p43, p21;
            p13[0] = p1[0] - p3[0];
            p13[1] = p1[1] - p3[1];
            p13[2] = p1[2] - p3[2];
            p43[0] = p4[0] - p3[0];
            p43[1] = p4[1] - p3[1];
            p43[2] = p4[2] - p3[2];
            if (mst::abs(p43[0]) < SMALL_NUM && 
                mst::abs(p43[1]) < SMALL_NUM && 
                mst::abs(p43[2]) < SMALL_NUM)
            {
                return false;
            }
            p21[0] = p2[0] - p1[0];
            p21[1] = p2[1] - p1[1];
            p21[2] = p2[2] - p1[2];
            if (mst::abs(p21[0]) < SMALL_NUM && 
                mst::abs(p21[1]) < SMALL_NUM && 
                mst::abs(p21[2]) < SMALL_NUM)
            { 
                return false;
            }

            double d1343 = p13[0] * p43[0] + p13[1] * p43[1] + p13[2] * p43[2];
            double d4321 = p43[0] * p21[0] + p43[1] * p21[1] + p43[2] * p21[2];
            double d1321 = p13[0] * p21[0] + p13[1] * p21[1] + p13[2] * p21[2];
            double d4343 = p43[0] * p43[0] + p43[1] * p43[1] + p43[2] * p43[2];
            double d2121 = p21[0] * p21[0] + p21[1] * p21[1] + p21[2] * p21[2];

            double denom = d2121 * d4343 - d4321 * d4321;
            if (mst::abs(denom) < SMALL_NUM)
            {
                return false;
            }

            double numer = d1343 * d4321 - d1321 * d4343;
            double mua = numer / denom;
            double mub = (d1343 + d4321 * mua) / d4343;
            pLine->set(mst::Vector3<Type>(p1[0] + mua * p21[0], p1[1] + mua * p21[1], p1[2] + mua * p21[2]),
                       mst::Vector3<Type>(p3[0] + mub * p43[0], p3[1] + mub * p43[1], p3[2] + mub * p43[2]));

            return true;
        }




private:
	Vector m_v[2];
};

/*! 
\typedef Line3f
\brief float specialisation of a 3D line
\ingroup mmllin
*/
typedef Line3<float> Line3f;

/*! 
\typedef Line3d
\brief double specialisation of a 3D line
\ingroup mmllin
*/
typedef Line3<double> Line3d;

/*! 
\fn template<class Type> std::ostream& operator<<(std::ostream &s, const mml::Line3<Type> &v)
\brief stream output operator of a 3D vector
\ingroup mmlio
*/
template<class Type> std::ostream& operator<<(std::ostream &s, const mml::Line3<Type> &v)
	{ s << v[0] << " " << v[1]; return s; }


}

#endif
